import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;

import javax.swing.BorderFactory;
import javax.swing.JPanel;

import Jama.Matrix;


public class Robot{

	//Top left corner of the robot (wheel 1)
	private double position_x;
	private double position_y;
	
	private double width;
	private double height;
	
	//Amount by which car is rotated
	private double theta;
	
	Sensor s1;
	Sensor s2;
	
	private Matrix kMatrix;
	private Matrix speed; //???
	
	public Robot(double initialX, double initialY, double k1Val, double k2Val, double k3Val, double k4Val)
	{	
		height = 20;
		width = 40;
		
		position_x = initialX;
		position_y = initialY;
		
		double [][] k = {{k1Val, k2Val},
						{k3Val, k4Val}};
		kMatrix = new Matrix(k);
		
		s1 = new Sensor(position_x+width, position_y);
		s2 = new Sensor(position_x, position_x+height);
		
		theta = 0;
		
		speed = new Matrix(2, 1);
	}
	
	public void setPosX(int x)
	{
		position_x = x;
	}
	
	public void setPosY(int y)
	{
		position_y=y;
	}
	
	public void setTheta(double t)
	{
		theta = t;
	}
	
	// set the speed using the given light
	// BE SURE TO PASS THE CLOSEST LIGHT
	public void setSpeed(Light light)
	{
		double [][] tmp = {{s1.output(light)},
				{s2.output(light)}};
		Matrix sense = new Matrix(tmp);
		
		speed = kMatrix.times(sense); // if we have a base speed, add it in here
	}
	
	// Calculate and set the location 1 (??) second in the future
	// Correct speed should be set b/f calling this
	public void calcLocation()
	{
		double speedOut, speedIn;
		int time = 2; // or 1000??
		
		// both have the same speed
		//if (Math.abs(speed.get(0, 0) - speed.get(1, 0)) < 0.5)
		//{
			// TODO: move in a straight line and return
		//	return;
		//}
		
		/*if (speed.get(0, 0) > speed.get(1, 0))
		{
			System.out.println("Out: "+speed.get(0, 0) + "In: "+speed.get(1, 0));
			speedOut = speed.get(0, 0);
			speedIn = speed.get(1, 0);
		}
		else
		{
			System.out.println("Out: "+speed.get(1, 0) + "In: "+speed.get(1, 0));
			speedOut = speed.get(1, 0);
			speedIn = speed.get(0, 0);
		}
		*/
		//position_x = (position_x + (width / 2) * ((speedOut + speedIn) / (speedOut - speedIn)) *
		//			(Math.sin((speedOut - speedIn) * (time / (double)width) + theta) - Math.sin(theta)));
		//position_y = (position_y + (width / 2) * ((speedOut + speedIn) / (speedOut - speedIn)) *
		//			(Math.cos((speedOut - speedIn) * (time / (double)width) + theta) - Math.cos(theta)));
		//position_x = position_x+;
		//position_y = position_y+;
		//Always positive???
		theta = (speed.get(0,0) - speed.get(1, 0)) * (time / (double)width) + theta; //speedOut - speedIn
		System.out.println("Theta: "+theta);
		System.out.println("Speed0: "+speed.get(0,0)+"Speed1: "+speed.get(1,0));
		System.out.println(position_x + " " + position_y);
	}
	
	public Matrix getKMatrix()
	{
		return kMatrix;
	}
	
	public double getPosX()
	{
		return position_x;
	}
	
	public double getPosY()
	{
		return position_y;
	}
	
	public double getTheta()
	{
		return theta;
	}
	
	public double getWidth()
	{
		return width;
	}
	
	public double getHeight()
	{
		return height;
	}
	
	public Sensor getSensor1()
	{
		return s1;
	}
	
	public Sensor getSensor2()
	{
		return s2;
	}
}
